// --------------------------------------------------------------------------------------------------------------------
// <copyright file="UpwardFacingCameraPoseEstimator.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction
{    
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.IO;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision.GroundTruthExtraction;

    /// <summary>
    /// Camera pose estimator class, which is responsible for estimating the camera pose using the planar pose, landmark tilt information.
    /// </summary>
    public class UpwardFacingCameraPoseEstimator : IDisposable
    {
        /// <summary>
        /// Track whether Dispose has been called.
        /// </summary>
        private bool disposed;

        /// <summary>
        /// The planar pose estimator.
        /// </summary>
        private ManagedPlanarPoseEstimator planarPoseEstimator;

        /// <summary>
        /// Camera CalibrationInfo which contains the camera intrinsic and extrinsic params.
        /// </summary>
        private CameraCalibrationInfo cameraCalibrationInfo;

        /// <summary>
        /// Landmark information which contains the list of landmark points.
        /// </summary>
        private LandmarksInfo landmarkInfo;

        /// <summary>
        /// Initializes a new instance of the UpwardFacingCameraPoseEstimator class
        /// </summary>
        /// <param name="robotConfigFolder">Path to th robot configuration folder</param>
        /// <param name="environmentConfigFolder">Path to the environment configuration folder</param>
        public unsafe UpwardFacingCameraPoseEstimator(string robotConfigFolder, string environmentConfigFolder)
        {
            this.cameraCalibrationInfo = DataLoader<CameraCalibrationInfo>.ImportData(Path.Combine(robotConfigFolder, CameraCalibrationInfo.Filename));
            if (this.cameraCalibrationInfo == null)
            {
                TraceOut.Warning(
                    TraceContexts.Test,
                    "{0} not found in calibration folder {1}, cannot perform camera pose estimation",
                    CameraCalibrationInfo.Filename,
                    robotConfigFolder);
            }
            else
            {
                fixed (double* colorIntrinsicParamsPtr = this.cameraCalibrationInfo.ColorIntrinsicParams)
                {
                    fixed (double* colorRDistortionParamsPtr = this.cameraCalibrationInfo.ColorRDistortionParams)
                    {
                        this.planarPoseEstimator = new ManagedPlanarPoseEstimator(
                            colorIntrinsicParamsPtr,
                                this.cameraCalibrationInfo.ColorIntrinsicParams.Length,
                            colorRDistortionParamsPtr,
                                this.cameraCalibrationInfo.ColorRDistortionParams.Length);
                    }
                }
            }

            this.landmarkInfo = DataLoader<LandmarksInfo>.ImportData(Path.Combine(environmentConfigFolder, LandmarksInfo.Filename));

            if (this.landmarkInfo == null)
            {
                TraceOut.Warning(
                    TraceContexts.Test,
                    "{0} not found in calibration folder {1}, cannot perform landmark tilt error correction or robot pose estimation",
                    LandmarksInfo.Filename,
                    environmentConfigFolder);
            }
        }

        /// <summary>
        /// Finalizes an instance of the <see cref="UpwardFacingCameraPoseEstimator" /> class.
        /// </summary>
        ~UpwardFacingCameraPoseEstimator()
        {
            this.Dispose(false);
        }

        /// <summary>
        /// Gets the number of landmarks in the landmarkInfo.
        /// </summary>
        public int LandmarkCount
        {
            get { return this.landmarkInfo == null ? 0 : this.landmarkInfo.LandmarkPoints.Count(); }
        }

        /// <summary>
        /// Gets a value indicating whether the calibration info is present
        /// </summary>
        public bool HasCalibrationInfo
        {
            get
            {
                return this.cameraCalibrationInfo != null;
            }
        }

        /// <summary>
        /// Transform planar coordinate to landmark coordinate.
        /// </summary>
        /// <param name="planarPose"> Planar pose</param>
        /// <returns>Pose in landmark coordinates</returns>
        public static Pose2D ConvertPlanarCoordinateToLandmarkCoordinate(Pose2D planarPose)
        {
            // Convert the planar coordinate to landmark coordinates. The x and y are swapped and y and heading are flipped.
            Pose2D pose = new Pose2D(-planarPose.Y, planarPose.X, -planarPose.Heading);
            double r = Math.Sqrt(pose.X * pose.X + pose.Y * pose.Y);
            double theta = Math.Atan2(pose.Y, pose.X) + pose.Heading;
            return new Pose2D(r * Math.Cos(theta), r * Math.Sin(theta), pose.Heading);
        }

        /// <summary>
        /// Calculate the camera pose from an image
        /// </summary>
        /// <param name="lumImage">The input image</param>
        /// <param name="cameraPoseOut">The calculated 2D camera pose</param>
        /// <param name="rotationsOut">The calculated camera rotations</param>
        /// <param name="translationsOut">The calculated camera translations</param>
        /// <returns>Boolean indicating success or failure</returns>
        public bool TryGetCameraPose(ImageFrameGray8 lumImage, out Pose2D cameraPoseOut, out Vector3 rotationsOut, out Vector3 translationsOut)
        {
            // Do not attempt to provide ground truth if the camera is not calibrated.
            if (this.cameraCalibrationInfo == null)
            {
                cameraPoseOut = new Pose2D(double.NaN, double.NaN, double.NaN);
                rotationsOut = new Vector3(double.NaN, double.NaN, double.NaN);
                translationsOut = new Vector3(double.NaN, double.NaN, double.NaN);
                return false;
            }

            double[] translations = new double[3];
            double[] rotations = new double[3];
            bool foundCheckerBoard = false;

            unsafe
            {
                fixed (double* translationsPtr = translations)
                fixed (double* rotationsPtr = rotations)
                {
                    foundCheckerBoard = this.planarPoseEstimator.EstimatePose(
                        lumImage,
                        translationsPtr,
                        (uint)translations.Length,
                        rotationsPtr,
                        (uint)rotations.Length);
                }
            }

            if (foundCheckerBoard)
            {
                cameraPoseOut = ConvertPlanarCoordinateToLandmarkCoordinate(new Pose2D(translations[0], translations[1], rotations[2]));
                rotationsOut = new Vector3(rotations[0], rotations[1], rotations[2]);
                translationsOut = new Vector3(translations[0], translations[1], translations[2]);
                return true;
            }
            else
            {
                cameraPoseOut = new Pose2D(double.NaN, double.NaN, double.NaN);
                rotationsOut = new Vector3(double.NaN, double.NaN, double.NaN);
                translationsOut = new Vector3(double.NaN, double.NaN, double.NaN);
                return false;
            }
        }

        /// <summary>
        /// Find the landmark info for a specified landmarkID
        /// </summary>
        /// <param name="landmarkId">The landmark Id</param>
        /// <returns>A landmark class with landmark pose and tilt information</returns>
        public Landmark LookupLandmark(int? landmarkId)
        {
            Landmark currentLandmarkPoint = null;
            if (this.landmarkInfo != null && this.landmarkInfo.LandmarkPoints != null &&
                landmarkId != null && this.landmarkInfo.LandmarkPoints.Length > landmarkId)
            {
                currentLandmarkPoint = this.landmarkInfo.LandmarkPoints[landmarkId.Value];
            }

            return currentLandmarkPoint;
        }

        /// <summary>
        /// Implement IDisposable
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Dispose resources.
        /// </summary>
        /// <param name="disposing">True if dispose is called by user code, false otherwise</param>
        protected virtual void Dispose(bool disposing)
        {
            // Check to see if Dispose has already been called. 
            if (!this.disposed)
            {
                if (disposing)
                {
                    if (this.planarPoseEstimator != null)
                    {
                        this.planarPoseEstimator.Dispose();
                        this.planarPoseEstimator = null;
                    }
                }

                this.disposed = true;
            }
        }
    }
}
